#include "SceneObject.hpp"

#include <GL/freeglut.h>
#include <GL/gl.h>

#include "siut\simd\Vec4f.hpp"
using siut::simd::Vec4f;


// NB! Check matrix mult and scoped_ptr

SceneObject::SceneObject()
{
  setIdentity(matrix_);
}

SceneObject::~SceneObject()
{
}

void SceneObject::render()
{
  glPushMatrix();
    //this->matWorld_.multMatrix();
    glMultMatrixf(&matrix_[0]);
	this->privateRender();
	
    for(std::vector<boost::shared_ptr<SceneObject> >::iterator it = children_.begin();
        it != children_.end(); it++)
        (*it)->render();
  glPopMatrix();
}

void SceneObject::update(double sslf)
{
  this->sslf_ = sslf;
  this->privateUpdate();
  for(std::vector<boost::shared_ptr<SceneObject> >::iterator it = children_.begin();
      it != children_.end(); it++)
      (*it)->update(sslf);
}

// \breif init self and children
void SceneObject::init()
{
  this->privateInit();
  for(std::vector<boost::shared_ptr<SceneObject> >::iterator it = children_.begin();
      it != children_.end(); it++)
      (*it)->init();
}

void SceneObject::addSubObject(boost::shared_ptr<SceneObject> newchild)
{
  children_.push_back(newchild);
}

void SceneObject::removeSubObject(const boost::shared_ptr<SceneObject> child)
{
  for(std::vector<boost::shared_ptr<SceneObject> >::iterator it = children_.begin();
      it != children_.end(); it++)
    if(*it == child)
    {
      children_.erase(it);
      break;
    }
}

void SceneObject::setPosition(const Vec3f& newPos)
{
	Vec4f posNow( c4( matrix_ ) );
	posNow *= -1;
	translate( matrix_, posNow[0], posNow[1], posNow[2] );
	translate( matrix_,  newPos[0], newPos[1], newPos[2] );	
}

const Vec3f SceneObject::getPos()
{
  Vec4f column4( c4( matrix_ ) );
  return Vec3f(column4[0], column4[1], column4[2] );
}


